#include "fly.h"

#define PWM_DEV_NAME "pwm3"  /* PWM设备名称 */
#define PWM_MOTOR1_CHANNEL 1 /* PWM通道 */
#define PWM_MOTOR2_CHANNEL 2 /* PWM通道 */
#define PWM_MOTOR3_CHANNEL 3 /* PWM通道 */
#define PWM_MOTOR4_CHANNEL 4 /* PWM通道 */

struct rt_device_pwm *pwm_dev; /* PWM设备句柄 */

void set_motor_pwm(rt_int32_t pwm1, rt_int32_t pwm2, rt_int32_t pwm3, rt_int32_t pwm4)
{
    rt_pwm_set(pwm_dev, PWM_MOTOR1_CHANNEL, MOTOR_PWM_PERIOD, pwm1);
    rt_pwm_set(pwm_dev, PWM_MOTOR2_CHANNEL, MOTOR_PWM_PERIOD, pwm2);
    rt_pwm_set(pwm_dev, PWM_MOTOR3_CHANNEL, MOTOR_PWM_PERIOD, pwm3);
    rt_pwm_set(pwm_dev, PWM_MOTOR4_CHANNEL, MOTOR_PWM_PERIOD, pwm4);
}

void motor_enable()
{
    rt_pwm_enable(pwm_dev, PWM_MOTOR1_CHANNEL);
    rt_pwm_enable(pwm_dev, PWM_MOTOR2_CHANNEL);
    rt_pwm_enable(pwm_dev, PWM_MOTOR3_CHANNEL);
    rt_pwm_enable(pwm_dev, PWM_MOTOR4_CHANNEL);
}

void motor_disable()
{
    rt_pwm_disable(pwm_dev, PWM_MOTOR1_CHANNEL);
    rt_pwm_disable(pwm_dev, PWM_MOTOR2_CHANNEL);
    rt_pwm_disable(pwm_dev, PWM_MOTOR3_CHANNEL);
    rt_pwm_disable(pwm_dev, PWM_MOTOR4_CHANNEL);
    info.is_unlock = 0;
}

void motor_lock()
{
    set_motor_pwm(0, 0, 0, 0);
    rt_thread_delay(100);
    motor_disable();
    info.is_unlock = 1;
}

void motor_unlock()
{
    motor_enable();
    set_motor_pwm(2000000, 2000000, 2000000, 2000000);
    rt_thread_delay(3000);
    set_motor_pwm(1000000, 1000000, 1000000, 1000000);
    info.is_unlock = 1;
}

int motor_init(void)
{
    /* 电机PWM输出 */
    pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
    if (pwm_dev == RT_NULL)
    {
        rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
        return RT_ERROR;
    }
    motor_enable();
    set_motor_pwm(0, 0, 0, 0);
    return 0;
}
INIT_APP_EXPORT(motor_init);
